#include "subctrl.h"
#include "adc.h"
#include "calibration.h"
#include "ds18b20.h"
#include "main.h"
#include "sys.h"
#include <math.h>
#include <stdint.h>
#include <string.h>

const static sub_control_flash_t kDefaultFlash = {
    .init = DEFAULT_INITIAL_VALUE,
    .ip = {192, 168, 9, 123},
};

sub_control_t subctrl = {NULL};
sub_control_flash_t flash = {NULL};

const static ds18b20_config_t kTempConfig[] = {
    {TEMP1_GPIO_Port, TEMP1_Pin, &subctrl.pa[0].temperature}, {TEMP2_GPIO_Port, TEMP2_Pin, &subctrl.pa[1].temperature},
    {TEMP3_GPIO_Port, TEMP3_Pin, &subctrl.pa[2].temperature}, {TEMP4_GPIO_Port, TEMP4_Pin, &subctrl.pa[3].temperature},
    {TEMP5_GPIO_Port, TEMP5_Pin, &subctrl.pa[4].temperature}, {TEMP6_GPIO_Port, TEMP6_Pin, &subctrl.pa[5].temperature},
    {TEMP7_GPIO_Port, TEMP7_Pin, &subctrl.pa[7].temperature},
};

const static pd_pin_index_t kPdConfig[] = {
    {PD1_GPIO_Port, PD1_Pin}, {PD2_GPIO_Port, PD2_Pin}, {PD3_GPIO_Port, PD3_Pin}, {PD4_GPIO_Port, PD4_Pin},
    {PD5_GPIO_Port, PD5_Pin}, {PD6_GPIO_Port, PD6_Pin}, {PD7_GPIO_Port, PD7_Pin},
};

#define ADC_SUM_NUM 10
#define ADC_CHANNEL_NUM 9

static uint16_t adc_raw_data[ADC_CHANNEL_NUM] = {NULL};

void Flash_Init(sub_control_flash_t *flash, const void *default_flash);
uint8_t SubCtrl_CheckCurrentDiff(float max, float min);
uint8_t SubCtrl_CheckTemperature(void);

sub_control_t *SubCtrl_Init(void)
{
    LOGI("SubCtrl init start");
    subctrl.frame_header = RS485_FRAME_HEADER;
    subctrl.header.init = DEFAULT_INITIAL_VALUE;
    Flash_Init(&flash, &kDefaultFlash);
    SubCtrl_LoadParams();
    SubCtrl_SetPower(0);
    DS18B20_Init(kTempConfig);

    HAL_ADCEx_Calibration_Start(&hadc1);
    HAL_ADC_Start_DMA(&hadc1, (uint32_t *)adc_raw_data, ADC_CHANNEL_NUM);
    LOGI("SubCtrl init success");
    return &subctrl;
}

void SubCtrl_SetOuput(switch_state_t state)
{
    for (uint8_t i = 0; i < kModuleNum; i++)
    {
        if (state == kON)
        {
            BIT_SET(subctrl.state.pa_enable, i);
            HAL_GPIO_WritePin(kPdConfig[i].port, kPdConfig[i].pin, GPIO_PIN_RESET);
            delay_ms(10); // Delay 10ms to prevent excessive inrush current when the amplifier module is powered on
                          // simultaneously.
        }
        else
        {
            BIT_CLEAR(subctrl.state.pa_enable, i);
            HAL_GPIO_WritePin(kPdConfig[i].port, kPdConfig[i].pin, GPIO_PIN_SET);
        }
    }
    if (state == kON)
        LED_OUTPUT_ON();
    else
        LED_OUTPUT_OFF();
    LOGI("Set PA output: %s", state == kON ? "ON" : "OFF");
}
void SubCtrl_SetPower(int power)
{
    if (power < 0 || (flash.config.forward.max > 0 && power > flash.config.forward.max))
    {
        LOGW("Invalid power value: %5d W", power);
        return;
    }
    subctrl.rf_power.target = power;
    LOGI("Set RF power: %5d W", subctrl.rf_power.target);
    SubCtrl_SetOuput(subctrl.rf_power.target > 0 ? kON : kOFF);
}

void SubCtrl_SetMaxForwardPower(int power)
{
#define RF_POWER_MAX 2000
    if (power < 0 || power > RF_POWER_MAX)
    {
        LOGW("Invalid max forward power value: %5d W", power);
        return;
    }
    flash.config.forward.max = power;
    LOGI("Set max forward power: %5d W", flash.config.forward.max);
}

void SubCtrl_SetForwardPoint(uint8_t index, int power)
{
    float total_current = 0.0f;
    for (uint8_t i = 0; i < kModuleNum; i++)
    {
        total_current += subctrl.pa[i].current;
    }
    if (CalibrationPointRecord(index, total_current, power) == 0)
    {
        LOGI("Set forward calibration point: index = %d, current = %f A, power = %d W", index, total_current, power);
    }
    else
    {
        LOGW("Invalid index: %d, should be in [0, %d], current = %f A", index, CALIBRATION_MAX_POINTS - 1,
             total_current);
    }
}

void SubCtrl_SetForwardCalibration(float a, float b, float c, float d)
{
    flash.config.forward.a = a;
    flash.config.forward.b = b;
    flash.config.forward.c = c;
    flash.config.forward.d = d;

    LOGI("Set forward calibration parameters: a = %.4f, b = %.4f, c = %.4f, d = %.4f", flash.config.forward.a,
         flash.config.forward.b, flash.config.forward.c, flash.config.forward.d);
}

void SubCtrl_GetForwardCalibration()
{
    cubic_param_t *param = CalibrationGetParam();
    if (param == NULL)
    {
        LOGW("No calibration data available");
        return;
    }
    LOGI("Calibration parameters: a = %.4f, b = %.4f, c = %.4f, d = %.4f", param->a, param->b, param->c, param->d);
}

void SubCtrl_StartForwardCalibration(void)
{
    CalibrationStartFit();
    SubCtrl_GetForwardCalibration();
}

void SubCtrl_ResetForwardCalibration(void)
{
    CalibrationReset();
    LOGI("Forward calibration reset");
}

void SubCtrl_SetAddress(uint8_t *ip)
{
    if (ip[3] < SUBCTRL_BASE_ADDRESS || ip[3] >= SUBCTRL_BASE_ADDRESS + kSubCtrlNum)
    {
        LOGW("Invalid address: %d, should be in [%d, %d]", ip[3], SUBCTRL_BASE_ADDRESS,
             SUBCTRL_BASE_ADDRESS + kSubCtrlNum - 1);
        return;
    }
    memcpy(subctrl.header.ip, ip, sizeof(subctrl.header.ip));
    LOGI("Set IP address: %d.%d.%d.%d", ip[0], ip[1], ip[2], ip[3]);
}

void SubCtrl_SaveParams(void)
{
    flash.init = DEFAULT_INITIAL_VALUE;
    memcpy(&flash.config.forward, CalibrationGetParam(), sizeof(cubic_param_t));
    memcpy(flash.ip, subctrl.header.ip, sizeof(subctrl.header.ip));
    if (Flash_Write(FLASH_START_ADDR, (uint32_t *)&flash, sizeof(sub_control_flash_t) / sizeof(uint32_t)))
    {
        LOGE("Flash write failed");
    }
    else
    {
        LOGI("Flash write success");
    }
}
void SubCtrl_LoadParams(void)
{
    CalibrationSetParam(&flash.config.forward);
    memcpy(subctrl.header.ip, flash.ip, sizeof(subctrl.header.ip));
    LOGI("Flash parameters loaded, ip: %d.%d.%d.%d", subctrl.header.ip[0], subctrl.header.ip[1], subctrl.header.ip[2],
         subctrl.header.ip[3]);
}

uint8_t Subctrl_CheckAddr(uint8_t ip)
{
    return ip == subctrl.header.ip[3];
}

void SubCtrl_Run(void)
{
#define RUN_INTERVAL 500
#define SAMPLE_INTERVAL 50

    static uint8_t index = 0;
    static uint16_t data_sample[ADC_SUM_NUM][ADC_CHANNEL_NUM] = {NULL};
    static float data_avg[ADC_CHANNEL_NUM] = {NULL};
    static uint32_t last_run_time = 0, last_sample_time = 0;

    float total_current = 0.0f;
    float max_current = 0.0f, min_current = 15.0f;

    if (HAL_GetTick() >= last_sample_time) // 50ms Sampling
    {
        last_sample_time = HAL_GetTick() + SAMPLE_INTERVAL;
        memcpy(data_sample[index++], adc_raw_data, sizeof(adc_raw_data));
        if (index >= ADC_SUM_NUM)
            index = 0;

        for (uint8_t i = 0; i < ADC_CHANNEL_NUM; i++)
        {
            data_avg[i] = 0.0f;
            for (uint8_t j = 0; j < ADC_SUM_NUM; j++)
            {
                data_avg[i] += data_sample[j][i];
            }
            data_avg[i] /= ADC_SUM_NUM;
        }
    }

    if (HAL_GetTick() >= last_run_time) // 500ms Running
    {
        last_run_time = HAL_GetTick() + RUN_INTERVAL;
        for (uint8_t i = 0; i < ADC_CHANNEL_NUM; i++)
        {
            if (i < kModuleNum)
            {
                subctrl.pa[i].current = data_avg[i] * (3.3f / 4095) / (SAMPLING_RESISTOR * MAX4080_GAIN);
                total_current += subctrl.pa[i].current;
                if (subctrl.pa[i].current > max_current)
                    max_current = subctrl.pa[i].current;
                if (subctrl.pa[i].current < min_current)
                    min_current = subctrl.pa[i].current;
            }
            /*  adc channel [0-6] for PA modules current
                adc channel [7] for RF power forward
                adc channel [8] for RF power reverse */
        }
        SubCtrl_CheckCurrentDiff(max_current, min_current);
        SubCtrl_CheckTemperature();
        subctrl.rf_power.forward = CalculateForwardPower(total_current);
        subctrl.rf_power.reverse = CalculateSWR(data_avg[7], data_avg[8]);
        LOGI("FwdVoltage: %.5f V, RevVoltage: %.5f V, SWR: %.5f", data_avg[7] * (3.3f / 4095),
             data_avg[8] * (3.3f / 4095), subctrl.rf_power.reverse);
    }
}

void Flash_Init(sub_control_flash_t *flash, const void *default_flash)
{
#if !ENABLE_DEBUG_MODE
    sub_control_flash_t temp;

    flash->init = DEFAULT_INITIAL_VALUE;
    Flash_Read(FLASH_START_ADDR, (uint32_t *)&temp, sizeof(sub_control_flash_t) / sizeof(uint32_t));
    if (temp.init != flash->init)
    {
        memcpy(flash, default_flash, sizeof(sub_control_flash_t));
        LOGI("Flash is initialized");
        if (Flash_Write(FLASH_START_ADDR, (uint32_t *)flash, sizeof(sub_control_flash_t) / sizeof(uint32_t)))
        {
            LOGE("Flash write failed");
        }
        else
        {
            LOGI("Flash write success");
        }
    }
    else
    {
        memcpy(flash, &temp, sizeof(sub_control_flash_t));
        LOGI("Read flash parameters");
    }
#else
    memcpy(flash, default_flash, sizeof(sub_control_flash_t));
#endif
}

uint8_t SubCtrl_CheckCurrentDiff(float max, float min)
{
#define MAX_CURRENT_DIFF 4.0f // 最大电流差值，单位A
    if (fabsf(max - min) > MAX_CURRENT_DIFF)
    {
        LED_CURRENT_ON();
        LOGW("Current difference too large: max = %.2f A, min = %.2f A", max, min);
        return 1;
    }
    else
    {
        LED_CURRENT_OFF();
        return 0;
    }
}

uint8_t SubCtrl_CheckTemperature(void)
{
#define MAX_TEMPERATURE 70.0f // Maximum temperature, unit ℃

    float temp = 0.0f;
    for (uint8_t i = 0; i < kModuleNum; i++)
    {
        if (temp < subctrl.pa[i].temperature)
            temp = subctrl.pa[i].temperature;
    }
    if (temp > MAX_TEMPERATURE)
    {
        LED_TEMP_ON();
        LOGW("Temperature too high: %.2f ℃", temp);
        return 1;
    }
    else
    {
        LED_TEMP_OFF();
        return 0;
    }
}